#! /home/a/anaconda3/envs/ros/bin/python
import rospy
from controller import Controller

def servo():
    rospy.init_node('unitree_gazebo_servo_py')
    robot_name = rospy.get_param('/robot_name')
    print('robot name:', robot_name)

    robot_controller = Controller(robot_name)
    robot_controller.motion_init()
    print('init finish.')

    rate = rospy.Rate(10) # 10hz
    while not rospy.is_shutdown():

        # robot_controller.sendServoCmd()
        rate.sleep()


if __name__ == '__main__':
    try:
        servo()
    except rospy.ROSInterruptException:
        pass
